import sys
# sys.path.append('/home/unitree/go1_guide/unitree_legged_sdk/lib/python/arm64')
sys.path.append(r'/home/pi/go1_guide/unitree_legged_sdk/lib/python/arm64')
import robot_interface as sdk

import unitree

class Unitree_Robot_High():
    def __init__(self):
        self.udp = sdk.UDP(unitree.HIGHLEVEL, 8080, "192.168.123.161", 8082)
        self.cmd = sdk.HighCmd()
        self.state = sdk.HighState()
        self.udp.InitCmdData(self.cmd)

        self.init_cmd()

    def init_cmd(self):
        self.cmd.mode = 0      # 0:idle, default stand      1:forced stand     2:walk continuously
        self.cmd.gaitType = 0
        self.cmd.speedLevel = 0
        self.cmd.footRaiseHeight = 0
        self.cmd.bodyHeight = 0
        self.cmd.euler = [0, 0, 0]
        self.cmd.velocity = [0, 0]
        self.cmd.yawSpeed = 0.0
        self.cmd.reserve = 0
    
    def send_UDP(self):
        self.udp.SetSend(self.cmd)
        self.udp.Send()
    
    def execute(self, mode=2, gaitType=0, speedLevel=0, 
                      footRaiseHeight=0, bodyHeight=0, 
                      euler=[0,0,0], velocity=[0,0], yawSpeed=0.0, reverve=0):
        self.init_cmd()
        self.cmd.mode = mode
        self.cmd.gaitType = gaitType
        self.cmd.speedLevel = speedLevel
        self.cmd.footRaiseHeight = footRaiseHeight
        self.cmd.bodyHeight = bodyHeight
        self.cmd.euler = euler
        self.cmd.velocity = velocity
        self.cmd.yawSpeed = yawSpeed
        self.cmd.reserve = reverve
        self.send_UDP()

    # def pose(self, roll, pitch, yaw, bodyHeight):
    #     self.init_cmd()
    #     self.cmd.mode = 1
    #     self.cmd.bodyHeight = bodyHeight
    #     self.cmd.yaw = yaw
    #     self.cmd.pitch = pitch
    #     self.cmd.roll = roll
    #     self.send_UDP()
    
    def getState(self):
        self.udp.Recv()
        self.udp.GetRecv(self.state)
        return self.state

